%%
% 实验三1（1）
K = 1000;
num = K * [1 1];
den = conv([1 0], conv([1 2], [1 17 4000]));
G = tf(num, den);
bode(G);
grid on;
hold on;

%%
% 实验三1（2）
K = 1000;
num = K * [1 1];
den = conv([1 0], conv([1 2], [1 17 4000]));
G = tf(num, den);
margin(G);
grid on;

%%
% 实验三1（3)
K = 2000;
num = K * [1 1];
den = conv([1 0], conv([1 2], [1 17 4000]));
G = tf(num, den);
bode(G);
grid on;
hold on;
legend('K=1000','2000')


%%
% 实验三2（1）

num = 10;
den = conv([1 5], [1 -1]);
G = tf(num, den);
nyquist(G);
grid on;


%%
% 实验三2（3）
num = 10;
den = conv([1 5], [1 -1]);
G = tf(num, den);
pzmap(feedback(G,1));
grid on;


%%
% 实验三3
%G(s)=2/(s+1)(8s+1)
num1=2;
den1=conv([2 1], [8 1]);
G1=tf(num1,den1);

%G(s)=200/(s+1)(10s+1)
num2=200;
den2=conv([1 0 0], conv([1 1], [10 1]));
G2=tf(num2,den2);

%G(s)=8(s/0.1 + 1)/(s+1)(s^2+s+1)(s/2+1)
num3=conv(8, [10 1]);
den3=conv([1 0], conv([1 1 1], [1/2 1]));
G3=tf(num3,den3);

%G(s)=10(s^2/400+s/10+1)/(s)(s+1)(s/0.1+1)
num4=conv(10, [1/400 1/10 1]);
den4=conv([1 0], conv([1 1], [10 1]));
G4=tf(num4,den4);

subplot(241)
nyquist(G1);
title('G1(s)=2/(s+1)(8s+1)Nyquist曲线');
subplot(245)
pzmap(feedback(G1,1));
title('G1(s)=2/(s+1)(8s+1)闭环系统的零极点分布图');

subplot(242)
nyquist(G2);
title('G2(s)=200/(s+1)(10s+1)Nyquist曲线');
subplot(246)
pzmap(feedback(G2,1));
title('G2(s)=200/(s+1)(10s+1)闭环系统的零极点分布图');

subplot(243)
nyquist(G3);
title('G3(s)=8(s/0.1 + 1)/(s+1)(s^2+s+1)(s/2+1)Nyquist曲线');
subplot(247)
pzmap(feedback(G3,1));
title('G3(s)=8(s/0.1 + 1)/(s+1)(s^2+s+1)(s/2+1)闭环系统的零极点分布图');

subplot(244)
nyquist(G4);
title('G4(s)=10(s^2/400+s/10+1)/(s)(s+1)(s/0.1+1)Nyquist曲线');
subplot(248)
pzmap(feedback(G4,1));
title('G4(s)=10(s^2/400+s/10+1)/(s)(s+1)(s/0.1+1)闭环系统的零极点分布图');



%%
% 实验三4
%G(s)=2/(s+1)(8s+1)
num1=2;
den1=conv([2 1], [8 1]);
G1=tf(num1,den1);

%G(s)=200/(s+1)(10s+1)
num2=200;
den2=conv([1 0 0], conv([1 1], [10 1]));
G2=tf(num2,den2);

%G(s)=8(s/0.1 + 1)/(s+1)(s^2+s+1)(s/2+1)
num3=conv(8, [10 1]);
den3=conv([1 0], conv([1 1 1], [1/2 1]));
G3=tf(num3,den3);

%G(s)=10(s^2/400+s/10+1)/(s)(s+1)(s/0.1+1)
num4=conv(10, [1/400 1/10 1]);
den4=conv([1 0], conv([1 1], [10 1]));
G4=tf(num4,den4);

subplot(141)
bode(G1);
title('G1(s)=2/(s+1)(8s+1)Bode图');
grid on;

subplot(142)
bode(G2);
title('G2(s)=200/(s+1)(10s+1)Bode图');
grid on;

subplot(143)
bode(G3);
title('G3(s)=8(s/0.1 + 1)/(s+1)(s^2+s+1)(s/2+1)Bode图');
grid on;

subplot(144)
bode(G4);
title('G4(s)=10(s^2/400+s/10+1)/(s)(s+1)(s/0.1+1)Bode图');
grid on;


%%
% 实验四1(2)
num = 10;
den = conv([1 0], [1 1]);
G = tf(num, den);
margin(G);
grid on;

%%
% 实验四1(3)
s = tf('s');
G = 10 / (s * (s + 1));
[mag, phase, w] = bode(G);
mag = squeeze(mag); 
[Gm, Pm] = margin(G);
DPm = 45;
MPm = DPm - Pm + 5;
MPm = MPm * pi / 180;
a = (1 + sin(MPm)) / (1 - sin(MPm));
adb = 20 * log10(mag);
am = 10 * log10(a);
wc = spline(adb, w, -am);
T = 1 / (wc * sqrt(a));
at = a * T;
Gc = tf([at 1], [T 1]);
disp(Gc)
Gh = G * Gc;
figure, margin(Gh);
grid on;


%%
% 实验四2(2)
%G(s)=K/s(0.1s+1)(0.2s+1)
%K=30

s=tf('s');
G=30/(s*(0.1*s+1)*(0.2*s+1));
margin(G);
grid on;

%%
% 实验四2(3)
%G(s)=K/s(0.1s+1)(0.2s+1)
%K=30

s = tf('s');
G = 30 / (s * (0.1 * s + 1) * (0.2 * s + 1));

[mag, phase, w] = bode(G);
%mag = squeeze(mag); % 将mag转换为向量
phase = squeeze(phase); % 将phase转换为向量
w = squeeze(w);

[Gm, Pm] = margin(G);
DPm = 40;
MPm = -180 + DPm + 5;
wc1 = spline(phase, w, MPm);
Gwc1 = spline(w, mag, wc1);
L = 20 * log10(Gwc1);
b = 10^(-1 * (L / 20));
T = 10 / (b * wc1);
Gc = tf([b * T, 1], [T, 1]);
disp(Gc)
figure;
margin(Gc * G);
grid on;

%%
%实验四3（2）
%G(s)=K/s(s+1)(0.25s+1)
%k=30

s=tf('s');
G=30/(s*(s+1)*(0.25*s+1));
[mag,phase,w]=bode(G);
figure(1)
margin(G);
grid on;

%%
%实验四3（3）
%G(s)=K/s(s+1)(0.25s+1)
%k=30

s=tf('s');
G=30/(s*(s+1)*(0.25*s+1));
Kv=dcgain(s*G);
K_adjust=30/Kv;
G=K_adjust*G;
[Gm,Pm,wg,wc]=margin(G);
b=0.1;
T1=50/(b*wc);
Gc1=tf([b*T1 1], [T1 1]);
disp(Gc1)
G1=Gc1*G;
[mag1,phase1,w1]=bode(G1);
mag1=squeeze(mag1);
w1=squeeze(w1);

wc2=4.5;
[Gm1, Pm1, wg1, wc1]=margin(G1);
mag2=spline(w1,mag1,wc2);
L=20*log10(mag2);
a=10^(-L/20);
T2=1/(wc2*sqrt(a));
at=a*T2;
Gc=tf([at 1], [T2 1]);
disp(Gc);
Gh=G1*Gc;
figure();
margin(Gh);

%%
%实验四4（1）
s = tf('s');
G = 40 / (s * (0.2*s + 1) * (0.0625*s + 1));
disp(G);
[Gm, Pm, wg, wc] = margin(G);

b = 0.15; 
T1 = 50 / (b * wc);

Gc1 = tf([b * T1 1], [T1 1]);
disp(Gc1);

G1 = Gc1 * G;
figure();
margin(G1);
grid on;

%%
% 实验四4（2）

s = tf('s');
G = 40 / (s * (0.2*s + 1) * (0.0625*s + 1));
disp(G);
[Gm, Pm, wg, wc] = margin(G);

b = 0.09; 
T1 = 50 / (b * wc);

Gc1 = tf([b * T1 1], [T1 1]);
disp(Gc1);
G1 = Gc1 * G;

% 计算校正后系统的频率响应
[mag1, phase1, w1] = bode(G1);
mag1 = squeeze(mag1);
w1 = squeeze(w1);

% 设计滞后校正器
wc2 = 4.5; 
[Gm1, Pm1, wg1, wc1] = margin(G1);
mag2 = spline(w1, mag1, wc2);
L = 20 * log10(mag2);
a = 10^(-L / 20);
T2 = 1 / (wc2 * sqrt(a));
at = a * T2;
Gc2 = tf([at 1], [T2 1]);
disp(Gc2);


Gh = G1 * Gc2;

figure(2);
margin(Gh);
grid on;
%% 
%实验五2（1）
G=tf(1,conv([0.14 0.9 1],[1 1]));
Kp=[0.1 0.6 1.2 2.5 3 10];
for i=1:length(Kp)
    GGc=feedback(Kp(i)*G, 1);
    step(GGc)
    hold on;
    grid on;
end
legend('Kp=0.1','Kp=0.6','Kp=1.2','Kp=2.5','Kp=3','Kp=10')

%% 
%实验五2（2）
G=tf(1,conv([0.14 0.9 1],[1 1]));
Kp=1;
Ti=[0.41,1,2,3,6];
for i=1:length(Ti)
    Gc=tf(Kp*[1,1/Ti(i)],[1 0]);
    GGc=feedback(Gc*G,1);
    step(GGc)
    hold on;
    grid on;
end
legend('Ti=0.41','Ti=1','Ti=2','Ti=3','Ti=6')

%%
%实验五2（3）
G=tf(1,conv([0.14 0.9 1],[1 1])); 
Kp=1; 
Ti=0.41; 
Td=[0.1 0.6 1 2]; 
for i=1:length(Td) 
Gc=tf(Kp*[Td(i)*Ti Ti 1],[Ti 0]); 
GGc=feedback(Gc*G,1); 
step(GGc) 
hold on; 
grid on; 
end 
legend('Td=0.1','Td=0.6','Td=1','Td=2')

%%
% 实验四4（1） - 改进后的串联超前校正装置设计
s = tf('s');
G0 = 40 / (s * (0.2*s + 1) * (0.0625*s + 1));

% 计算当前系统的幅值裕量和相角裕量
[Gm, Pm, wg, wc] = margin(G0);
Gm_dB = 20*log10(Gm);
fprintf('当前系统的幅值裕量 (Gm): %.2f dB\n', Gm_dB);
fprintf('当前系统的相角裕量 (Pm): %.2f°\n', Pm);

% 设计目标
desired_phase_margin = 30; % 30°
desired_gain_margin_min = 10; % 10 dB
desired_gain_margin_max = 12; % 12 dB

% 计算相角缺口
phase_deficit = desired_phase_margin - Pm;
if phase_deficit <= 0
    disp('当前系统的相角裕量已满足或超过要求，无需补偿。');
else
    % 计算所需的最大相位补偿（phi_max）
    phi_max = phase_deficit; % 不再增加额外的5°
    
    if phi_max > 60
        error('所需的相位补偿过大，可能无法实现。');
    end
    
    % 计算alpha
    alpha = (1 - sind(phi_max)) / (1 + sind(phi_max));
    
    % 选择校正频率为当前相位交叉频率
    omega_lead = wc;
    
    % 计算零点和极点
    zc = omega_lead / sqrt(alpha);
    pc = alpha * zc;
    
    % 设计超前校正器传递函数
    Gc1 = (s + zc) / (s + pc);
    
    % 计算在omega_lead处的增益，以调整补偿器增益K
    [mag, ~] = bode(Gc1 * G0, omega_lead);
    mag = squeeze(mag);
    K = 1 / mag;
    
    % 调整补偿器增益
    Gc1 = K * Gc1;
    
    % 校正后的开环传递函数
    G1 = Gc1 * G0;
    
    % 重新计算裕度
    [Gm1, Pm1, wg1, wc1] = margin(G1);
    Gm1_dB = 20*log10(Gm1);
    
    fprintf('校正后系统的幅值裕量 (Gm1): %.2f dB\n', Gm1_dB);
    fprintf('校正后系统的相角裕量 (Pm1): %.2f°\n', Pm1);
    
    % 检查是否满足要求
    if (Pm1 >= desired_phase_margin) && (Gm1_dB >= desired_gain_margin_min) && (Gm1_dB <= desired_gain_margin_max)
        disp('超前校正装置设计满足要求。');
    else
        disp('超前校正装置设计不满足要求，需要进一步调整。');
        % 可尝试调整phi_max或omega_lead
    end
    
    % 显示补偿器传递函数
    disp('超前校正器传递函数 Gc1:');
    disp(Gc1);
    
    % 绘制校正后系统的相角裕度图
    figure;
    margin(G1);
    grid on;
    title('校正后系统的相角裕度图');
end

%%
s = tf('s');
G = 40 / (s * (0.2*s + 1) * (0.0625*s + 1));
% 计算增益、相位和频率响应
[mag, phase, w] = bode(G);
mag = squeeze(mag);
% 绘制系统的边界图
% 获取增益裕度、相位裕度等
[Gm, Pm, wg, wc] = margin(G);
% 设置控制器参数
b = 0.1; 
T1 = 50 / (b * wc);
% 设计控制器
Gc1 = tf([b * T1 1], [T1 1]);
% 开环传递函数
G1 = Gc1 * G;
% 绘制开环系统的边界图
[mag1, phase1, w1] = bode(G1);
% 选择新的截止频率 wc2
wc2 = 5.0;
% 获取新的增益裕度和相位裕度
[Gm1, Pm1, wg1, wc1] = margin(G1);
% 用样条插值计算在 wc2 频率下的幅度
mag2 = spline(w1, mag1, wc2);
% 计算L 和 a
L = 20 * log10(mag2);
a = 10^(-L / 20);
% 计算T2 和 at
T2 = 1 / (wc2 * sqrt(a));  % 注意这里修改了 sprt 为 sqrt
at = a * T2;
% 设计新的控制器 Gc
Gc = tf([at 1], [T2 1]);
% 闭环系统
Gh = Gc * G1;
% 绘制闭环系统的边界图
Gcc = Gc1 * G;
figure();
margin(Gh);
grid;



